#! /usr/bin/env python
# -*- coding: utf-8 -*
import face_recognition
import cv2
import numpy as np
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from std_msgs.msg import String
class Face_Rec():
    def __init__(self):
        rospy.init_node("image_cature") # 初始化节点
        self.video_capture = cv2.VideoCapture(6) # 定义摄像头0-front 2-behind 6jixiebide 6/4wozijide
        self.pub = rospy.Publisher("/camera/image", Image, queue_size=1) # 设置发布话题名、类型、设置队列个数       	self.pub = rospy.Publisher("/camera/image", Image, queue_size=1) # 设置发布话题名、类型、设置队列个数  
        self.known_face_encodings = list()
        self.known_face_names = list()
    def face_learning(self,dir,name):
        """加载图像并学习如何识别它，并添加到已知人脸库"""
        new_image = face_recognition.load_image_file(dir)
        new_face_encoding = face_recognition.face_encodings(new_image)[0]
        self.known_face_encodings.append(new_face_encoding)
        self.known_face_names.append(name) # 添加人脸名称

    def pub_img(self, img):
        """把图像发布到话题"""
        bridgr = CvBridge() 
        img = bridgr.cv2_to_imgmsg(img,"bgr8") # 把OpenCV图像转换为ROS消息
        self.pub.publish(img) # 发布图像到话题

    def face_rec(self):
	face_locations = [] # 检测到的未知人脸列表
        face_encodings = [] # 未知人脸编码列表
        face_names = [] # 实时标签列表列表
        process_this_frame = True 
        self.face_learning("/home/bobac3/ros_workspace/src/face_rec/image/xtq.jpg","xtq") # 学习的人脸图片位置，及标签名称
	self.face_learning("/home/bobac3/ros_workspace/src/face_rec/image/yyr.jpg","yyr") # 学习的人脸图片位置，及标签名称
	self.face_learning("/home/bobac3/ros_workspace/src/face_rec/image/zrz.jpg","zrz") # 学习的人脸图片位置，及标签名称
	self.face_learning("/home/bobac3/ros_workspace/src/face_rec/image/hld.jpg","hld") # 学习的人脸图片位置，及标签名称
	self.face_learning("/home/bobac3/ros_workspace/src/face_rec/image/ljx.jpg","ljx") # 学习的人脸图片位置，及标签名称
	
        while not rospy.is_shutdown():
	    name = ''
            ret, frame = self.video_capture.read()
            # 将视频帧大小调整为1/4大小，以加快人脸识别处理速度
            small_frame = cv2.resize(frame, (0, 0), fx=0.25, fy=0.25)
            # 将图像从 BGR 颜色（OpenCV 使用）转换为 RGB 颜色（face_recognition使用）
            rgb_small_frame = small_frame[:,:,::-1]
            if process_this_frame == True: # 间隔
                # 查找当前视频帧中的所有人脸
                face_locations = face_recognition.face_locations(rgb_small_frame)
                # 把查找到人脸进行编码
                face_encodings = face_recognition.face_encodings(rgb_small_frame,face_locations)
                face_names = []
                print("Detecting the number of faces:",len(face_encodings))
                for face_encoding in face_encodings:
                    # 将检测到的人脸和已知人脸库中的图片比较
                    name = "Unknown"
                    # 计算检测到的人脸和已知人脸的误差
                    face_distances = face_recognition.face_distance(self.known_face_encodings,face_encoding)
                    matches = face_recognition.compare_faces(self.known_face_encodings, face_encoding)
                    # 得到误差最小的人脸在已知列表中的位置
                    best_match_index = np.argmin(face_distances)
                    if matches[best_match_index]: # 如果对比结果为True
                        name =self.known_face_names[best_match_index] # 为检测到的人脸做标签
		    face_names.append(name)
            process_this_frame = not process_this_frame
###################################################################################
	    pub=rospy.Publisher("mingzi",String,queue_size=10)
###################################################################################
	    rate=rospy.Rate(20)
	    
	    msg=String()
	    msg.data=name
	    pub.publish(msg)
            for (top, right, bottom, left), name in zip(face_locations, face_names):
                # 在面部画一个框，放大备份人脸位置，因为我们检测到的帧被缩放到1/4大小
                top *= 4
                right *=4
                bottom *=4
                left *=4
                cv2.rectangle(frame, (left,top),(right,bottom),(0,0,255),2)
                # 在人脸下方写上标签
                cv2.rectangle(frame, (left, bottom - 35), (right, bottom), (0, 0, 255), cv2.FILLED)
                font = cv2.FONT_HERSHEY_DUPLEX
                cv2.putText(frame,name,(left + 6, bottom - 6), font, 1.0,(255,255,255), 1)
            self.pub_img(frame)
        self.video_capture.release()
if __name__=="__main__":
   	Face_Rec().face_rec()
